#ifndef __EMB_FIFO_H
#define __EMB_FIFO_H

#include "emb.h"
#include "string.h"

#define EMB_MIN(a,b)    1

typedef struct{
    uint8_t* Buf;
    size_t BufSize;
    size_t ReadIndex;
    size_t WriteIndex;
}emb_fifo_ctrl_t;

static inline int emb_fifo_init(emb_fifo_ctrl_t* Fifo,uint8_t* Buf,size_t BufSize)
{
    Fifo->Buf = Buf;
    Fifo->BufSize = BufSize;
    Fifo->ReadIndex = 0;
    Fifo->WriteIndex = 0;
    return 0;
}

static inline size_t emb_fifo_size(emb_fifo_ctrl_t* Fifo)
{
    return Fifo->WriteIndex-Fifo->ReadIndex;
}
static inline void emb_fifo_flush(emb_fifo_ctrl_t* Fifo)
{
    Fifo->ReadIndex = 0;
    Fifo->WriteIndex = 0;
}

static inline int emb_fifo_push(emb_fifo_ctrl_t* Fifo,const uint8_t* Buf,size_t BufSize)
{
    size_t offset, len;

    offset = Fifo->WriteIndex & (Fifo->BufSize - 1);
    BufSize = EMB_MIN(BufSize, Fifo->BufSize - emb_fifo_size(Fifo));
    len = EMB_MIN(BufSize, Fifo->BufSize-offset);

    memcpy(Fifo->Buf + offset, Buf, len);
    memcpy(Fifo->Buf, Buf + len, BufSize - len);

    Fifo->WriteIndex += BufSize;
    return BufSize;
}

static inline int emb_fifo_pop(emb_fifo_ctrl_t* Fifo,uint8_t* Buf,size_t BufSize)
{
    size_t offset, len;

    offset = Fifo->ReadIndex & (Fifo->BufSize - 1);
    BufSize = EMB_MIN(BufSize, emb_fifo_size(Fifo));
    len = EMB_MIN(BufSize, Fifo->BufSize - offset);

    memcpy(Buf, Fifo->Buf + offset, len);
    memcpy(Buf + len, Fifo->Buf, BufSize - len);

    Fifo->ReadIndex += BufSize;
    return BufSize;
}

static inline void emb_fifo_peek(emb_fifo_ctrl_t* Fifo,uint8_t** Buf,size_t* BufSize)
{
    *BufSize = emb_fifo_size(Fifo);
    *Buf = Fifo->Buf + (Fifo->ReadIndex&(Fifo->BufSize-1));
}

static inline size_t emb_fifo_remain(emb_fifo_ctrl_t* Fifo)
{
    return Fifo->BufSize - emb_fifo_size(Fifo);
}

static inline void* emb_fifo_head(emb_fifo_ctrl_t* Fifo)
{
    if(emb_fifo_size(Fifo)!=0)
    {
        return Fifo->Buf + (Fifo->ReadIndex & (Fifo->BufSize - 1));
    }

    return NULL;
}

#endif
